using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Dss.Core.Attributes;

namespace Diversity.Robotics.Navigation.GridSlamService
{
    /// <summary>
    /// The GridSlam State
    /// </summary>
    [DataContract()]
    public class GridSlamState
    {
        private RobotState _robotState;
        private int _numParticles = 100;

        [DataMember]
        public int NumParticles
        {
            get { return _numParticles; }
            set { _numParticles = value; }
        }
        private int _mapWidth = 100;

        [DataMember]
        public int MapWidth
        {
            get { return _mapWidth; }
            set { _mapWidth = value; }
        }
        private int _mapHeight = 100;

        [DataMember]
        public int MapHeight
        {
            get { return _mapHeight; }
            set { _mapHeight = value; }
        }
        private double _cmPerPixel = 10;

        [DataMember]
        public double CmPerPixel
        {
            get { return _cmPerPixel; }
            set { _cmPerPixel = value; }
        }

        [DataMember]
        public RobotState RobotState
        {
            get { return _robotState; }
            set { _robotState = value; }
        }

        private List<SensorTypeState> _sensorTypes;

        [DataMember]
        public List<SensorTypeState> SensorTypes
        {
            get { return _sensorTypes; }
            set { _sensorTypes = value; }
        }
        private List<BeamModelState> _beamModelStates;

        [DataMember]
        public List<BeamModelState> BeamModelStates
        {
            get { return _beamModelStates; }
            set { _beamModelStates = value; }
        }

        private List<ConeModelState> _coneModelStates;
        [DataMember]
        public List<ConeModelState> ConeModelStates
        {
            get { return _coneModelStates; }
            set { _coneModelStates = value; }
        }

    }

    [DataContract()]
    public class RobotState
    {
        private List<ServoState> _servos;
        private double _width = 15;

        [DataMember]
        public double Width
        {
            get { return _width; }
            set { _width = value; }
        }
        private double _length = 15;

        [DataMember]
        public double Length
        {
            get { return _length; }
            set { _length = value; }
        }

        [DataMember]
        public Pose2DState Pose2DIncoming = new Pose2DState();

        [DataMember]
        public Pose2DState Pose2DBestEstimate = new Pose2DState();

        [DataMember]
        public List<ServoState> Servos
        {
            get { return _servos; }
            set { _servos = value; }
        }
        private List<SensorState> _sensors;

        [DataMember]
        public List<SensorState> Sensors
        {
            get { return _sensors; }
            set { _sensors = value; }
        }

        private MotionModelState _motionModel;

        [DataMember]
        public MotionModelState MotionModel
        {
            get { return _motionModel; }
            set { _motionModel = value;}
        }
    }

    [DataContract()]
    [DataMemberConstructor]
    public class MotionModelState
    {
        double _vErr1;
        [DataMember]
        public double VErr1
        {
            get { return _vErr1; }
            set { _vErr1 = value; }
        }

        double _vErr2;
        [DataMember]
        public double VErr2
        {
            get { return _vErr2; }
            set { _vErr2 = value; }
        }

        double _rErr1;
        [DataMember]
        public double RErr1
        {
            get { return _rErr1; }
            set { _rErr1 = value; }
        }

        double _rErr2;
        [DataMember]
        public double RErr2
        {
            get { return _rErr2; }
            set { _rErr2 = value; }
        }

        public MotionModelState()
        {
        }

        public MotionModelState(double vErr1, double vErr2, double rErr1, double rErr2)
        {
            _vErr1 = vErr1;
            _vErr2 = vErr2;
            _rErr1 = rErr1;
            _rErr2 = rErr2;
        }
    }

    [DataContract]
    [DataMemberConstructor]
    public class Pose2DState
    {
        private double _x = 0;
        [DataMember]
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }
        private double _y = 0;

        [DataMember]
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }
        private double _theta = 0;

        [DataMember]
        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }

        public Pose2DState()
        {
        }

        public Pose2DState(double x, double y, double theta)
        {
            this._x = x;
            this._y = y;
            this._theta = theta;
        }

        public Pose2DState(Pose2D pose2D)
        {
            this._x = pose2D.X;
            this._y = pose2D.Y;
            this._theta = pose2D.Theta;
        }

        public Pose2D ToPose2D()
        {
            return new Pose2D(_x,_y,_theta);
        }
    }

    [DataContract()]
    public class SensorState
    {
        private string _name = "New Sensor";
        [DataMember]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }
        private int _id = 0;
        [DataMember]
        public int Id
        {
            get { return _id; }
            set { _id = value; }
        }
        private double _x = 0;
        [DataMember]
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }
        private double _y = 0;
        [DataMember]
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }
        private double _theta = 0;
        [DataMember]
        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }
        private string _sensorTypeName = "Undefined";
        [DataMember]
        public string SensorTypeName
        {
            get { return _sensorTypeName; }
            set { _sensorTypeName = value; }
        }
        private int _sensorTypeId = 0;
        [DataMember]
        public int SensorTypeId
        {
            get { return _sensorTypeId; }
            set { _sensorTypeId = value; }
        }
    }

    [DataContract()]
    public class ServoState
    {
        private string _name;
        [DataMember]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }
        private int _id;
        [DataMember]
        public int Id
        {
            get { return _id; }
            set { _id = value; }
        }
        private double _x;
        [DataMember]
        public double X
        {
            get { return _x; }
            set { _x = value; }
        }
        private double _y;
        [DataMember]
        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }
        private double _theta;
        [DataMember]
        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }
    }

    [DataContract()]
    public class BeamModelState
    {
        private string _name = "Default Beam Model";
        [DataMember]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }

        private int _id = 0;
        [DataMember]
        public int Id
        {
            get { return _id; }
            set { _id = value; }
        }
        private double _hitVariance = 80;
        [DataMember]
        public double HitVariance
        {
            get { return _hitVariance; }
            set { _hitVariance = value; }
        }
        private double _shortDecay = 0.01;
        [DataMember]
        public double ShortDecay
        {
            get { return _shortDecay; }
            set { _shortDecay = value; }
        }
        private double _maxRange = 200; // cm
        [DataMember]
        public double MaxRange
        {
            get { return _maxRange; }
            set { _maxRange = value; }
        }
        private double _mixHit = 0.33;
        [DataMember]
        public double MixHit
        {
            get { return _mixHit; }
            set { _mixHit = value; }
        }
        private double _mixShort = 0.35;
        [DataMember]
        public double MixShort
        {
            get { return _mixShort; }
            set { _mixShort = value; }
        }
        private double _mixRandom = 0.3;
        [DataMember]
        public double MixRandom
        {
            get { return _mixRandom; }
            set { _mixRandom = value; }
        }
        private double _mixFailure = 0.02;
        [DataMember]
        public double MixFailure
        {
            get { return _mixFailure; }
            set { _mixFailure = value; }
        }
        
    }

    [DataContract()]
    public class ConeModelState
    {
        private string _name = "Default Cone Model";
        [DataMember]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }
        private int _id = 0;
        [DataMember]
        public int Id
        {
            get { return _id; }
            set { _id = value; }
        }
        private double _maxRange = 200;
        [DataMember]
        public double MaxRange
        {
            get { return _maxRange; }
            set { _maxRange = value; }
        }
        private double _coneWidth = 0.5;
        [DataMember]
        public double ConeWidth
        {
            get { return _coneWidth; }
            set { _coneWidth = value; }
        }
        private double _obstacleSize = 2;
        [DataMember]
        public double ObstacleSize
        {
            get { return _obstacleSize; }
            set { _obstacleSize = value; }
        }
    }

    [DataContract()]
    public class SensorTypeState
    {
        private string _name;
        [DataMember]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }
        private int _id;
        [DataMember]
        public int Id
        {
            get { return _id; }
            set { _id = value; }
        }
        private string _coneModelName;
        [DataMember]
        public string ConeModelName
        {
            get { return _coneModelName; }
            set { _coneModelName = value; }
        }
        private int _coneModelId;
        [DataMember]
        public int ConeModelId
        {
            get { return _coneModelId; }
            set { _coneModelId = value; }
        }
        private string _beamModelName;
        [DataMember]
        public string BeamModelName
        {
            get { return _beamModelName; }
            set { _beamModelName = value; }
        }
        private int _beamModelId;
        [DataMember]
        public int BeamModelId
        {
            get { return _beamModelId; }
            set { _beamModelId = value; }
        }
    }


}
